import numpy as np
import math

def magnitude(vector):
   return np.sqrt(np.dot(np.array(vector),np.array(vector)))

def norm(vector):
   return np.array(vector)/magnitude(np.array(vector))

def line_ray_intersection_point(ray_origin, ray_direction, point1, point2):
    """
    >>> # Line segment
    >>> z1 = (0,0)
    >>> z2 = (10, 10)
    >>>
    >>> # Test ray 1 -- intersecting ray
    >>> r = (0, 5)
    >>> d = norm((1,0))
    >>> len(line_ray_intersection_point(r,d,z1,z2)) == 1
    True
    >>> # Test ray 2 -- intersecting ray
    >>> r = (5, 0)
    >>> d = norm((0,1))
    >>> len(line_ray_intersection_point(r,d,z1,z2)) == 1
    True
    >>> # Test ray 3 -- intersecting perpendicular ray
    >>> r0 = (0,10)
    >>> r1 = (10,0)
    >>> d = norm(np.array(r1)-np.array(r0))
    >>> len(line_ray_intersection_point(r0,d,z1,z2)) == 1
    True
    >>> # Test ray 4 -- intersecting perpendicular ray
    >>> r0 = (0, 10)
    >>> r1 = (10, 0)
    >>> d = norm(np.array(r0)-np.array(r1))
    >>> len(line_ray_intersection_point(r1,d,z1,z2)) == 1
    True
    >>> # Test ray 5 -- non intersecting anti-parallel ray
    >>> r = (-2, 0)
    >>> d = norm(np.array(z1)-np.array(z2))
    >>> len(line_ray_intersection_point(r,d,z1,z2)) == 0
    True
    >>> # Test ray 6 --intersecting perpendicular ray
    >>> r = (-2, 0)
    >>> d = norm(np.array(z1)-np.array(z2))
    >>> len(line_ray_intersection_point(r,d,z1,z2)) == 0
    True
    """
    # Convert to numpy arrays
    ray_origin = np.array(ray_origin, dtype=np.float)
    ray_direction = np.array(norm(ray_direction), dtype=np.float)
    point1 = np.array(point1, dtype=np.float)
    point2 = np.array(point2, dtype=np.float)
    # print(ray_origin, ray_direction, point1, point2)

    # Ray-Line Segment Intersection Test in 2D
    # http://bit.ly/1CoxdrG
    v1 = ray_origin - point1
    v2 = point2 - point1
    v3 = np.array([-ray_direction[1], ray_direction[0]])

    tmp = np.dot(v2, v3)
    if (tmp==0): return []

    t1 = np.cross(v2, v1) / tmp
    t2 = np.dot(v1, v3) / tmp

    if t1 >= 0.0 and t2 >= 0.0 and t2 <= 1.0:
        # print("dot=",ray_origin + t1 * ray_direction)
        return [ray_origin + t1 * ray_direction]
    return []

def calc_distance(x1,y1,x2,y2):
    diff_x = math.fabs(x1-x2)
    diff_y = math.fabs(y1-y2)

    dist = (diff_x**2 + diff_y**2)**0.5
    return dist

def calc_conn_distance(irs_center,irs_dir,sim_map):
    irs_p = (irs_center.x,irs_center.y)
    cx,cy = irs_center.x,irs_center.y

    mi_dis=0
    has_inter = False
    inter_point = None
    for seg in sim_map.segments():
        p1=seg[0]
        p2=seg[1]
        # print(irs_p,irs_dir,p1,p2)
        inters = line_ray_intersection_point(irs_p,irs_dir,p1,p2)
        if (len(inters)==1):
            ix,iy = inters[0]
            dis = calc_distance(cx,cy,ix,iy)
            # print(p1,p2,(ix,iy),"calc=",dis)
            if (not has_inter):
                has_inter = True
                mi_dis = dis
                inter_point = (ix,iy)
            else:
                if (dis<mi_dis):
                    mi_dis = dis
                    inter_point = (ix,iy)
    return has_inter,mi_dis,inter_point

def sensor_distance_generate(irs_pos,sim_map):
    calc_state = True
    state,F,Fp = calc_conn_distance(irs_pos,irs_pos.F_dir(),sim_map)
    calc_state = calc_state and state
    state,B,Bp = calc_conn_distance(irs_pos,irs_pos.B_dir(),sim_map)
    calc_state = calc_state and state
    state,L,Lp = calc_conn_distance(irs_pos,irs_pos.L_dir(),sim_map)
    calc_state = calc_state and state
    state,R,Rp = calc_conn_distance(irs_pos,irs_pos.R_dir(),sim_map)
    calc_state = calc_state and state
    
    inter_dis = [F,B,L,R]
    inter_ps = [Fp,Bp,Lp,Rp]
    
    # state,P,Pp = simulate_engine.engine_conn.calc_conn_distance(
    #     irs_pos,irs_pos.R_dir(),sim_map)
    # assert(state)
    
    # inter_dis = [P]*4
    # inter_ps = [Pp]*4

    return calc_state,inter_dis,inter_ps


if __name__ == "__main__":
    z1 = (0, 2400)
    z2 = (2400, 2400)
    r = (600,600)
    d = norm((1,0))
    ret = line_ray_intersection_point(r,d,z1,z2)

    print(ret)
    p = ret[0]
    print(p)
    input()